#include "Framework\Util\Array.h"
#include "nvec.h"
#include "RenderableTask.h"
#include "Mesh.h"
#include "SimEvents.h"
#include "Simulator.h"
#include "AStar.h"


AITask*			gAITask=null;

void	ASNode::getDescription(Dictionary&	dict){
	describe(ASNode,centroid);
	describe(ASNode,islandIndex);
	describe(ASNode,visited);
}

void	ASNode::detach(ASNode*	from){
	if(!from){
		for(ASNodeIterator	it=links.begin();it!=links.end();it++){
			for(ASNodeIterator	it2=(*it)->links.begin();it2!=(*it)->links.end();it2++){
				if((*it2)==this){
					(*it)->links.erase(it2);
					break;
				}
			}
		}
		links.clear();
		return;
	}

	for(ASNodeIterator	it=from->links.begin();it!=from->links.end();it++){
		if((*it)==this){from->links.erase(it);break;}
	}

	for(ASNodeIterator	it=links.begin();it!=links.end();it++){
		if((*it)==from){links.erase(it);break;}
	}

	//from->links.remove(this);
	//links.remove(from);
}

void	ASNode::linkTo(ASNode*	to){
	for(ASNodeIterator	an=to->links.begin();an!=to->links.end();an++)
	{
		massert((*an)!=this);
	}
	for(ASNodeIterator	an=links.begin();an!=links.end();an++)
	{
		massert((*an)!=to);
	}
	links.push_back(to);
	to->links.push_back(this);
}

ASNode::ASNode(){
	visited=false;
}

ASNode::~ASNode(){
	detach();
}

f32	ASSearch::estimateCost(ASNode*	from,ASNode*	to){
	return (to->centroid-from->centroid).length();
}

ASNodeVisitor*	ASSearch::visit(ASNode*	node,ASNode*	target,ASNodeVisitor*	parent){
	ASNodeVisitor*	v=visitorPool.alloc();
	v->node=node;
	v->costToTarget=estimateCost(node,target);
	v->costToStart=parent?parent->costToStart+estimateCost(parent->node,node):0.0f;
	v->totalCost=v->costToStart+v->costToTarget;
	v->parent=parent;
	newNodes.push_back(v);
	newMap[node]=v;
	return v;
}

bool	ASSearch::expand(){
	for(ASNodeSetIterator it=openSet.begin();it!=openSet.end();it++){
		ASNodeVisitor&	nv=(**it);
		for(ASNodeIterator ic=nv.node->links.begin();ic!=nv.node->links.end();ic++)
			if(openMap.find(*ic)==openMap.end()
				&&closedMap.find(*ic)==closedMap.end()
				&&newMap.find(*ic)==newMap.end())//This is probably redundant
				if((*ic)->islandIndex==target->islandIndex)
					visit((*ic),target,*it);
		break;
	}
	for(ASNodeSetIterator oi=openSet.begin();oi!=openSet.end();){
		closedMap[(*oi)->node]=*oi;	//Only expand the most promising node...
		closedSet.insert(*oi);
		if(openMap.find((*oi)->node)!=openMap.end())
			openMap.erase(openMap.find((*oi)->node));
		oi=openSet.erase(openSet.find(*oi));
		break;
	}
	newMap.clear();
	for(list<ASNodeVisitor*>::iterator	it=newNodes.begin();it!=newNodes.end();it++){
		openSet.insert(*it);
		openMap[(*it)->node]=(*it);
	}
	newNodes.clear();
	if(openSet.begin()!=openSet.end()){
		ASNodeVisitor*	v=*openSet.begin();
		if(v->node==target){
			while(v){path.push_front(v);v=v->parent;}
			return true;
		}
	}
	return false;
}

bool	ASSearch::find(){
	while(search());
	if(path.size())return true;
	return false;
}

bool	ASSearch::search(){
	if(!searching)return false;
	if(expand()||(openSet.size()==0)){searching=false;return false;} //Path not found.
	return true;
}

void	ASSearch::startSearch(ASNode*	startNode,ASNode*	_target){
	visitorPool.reset();
	closedMap.clear();
	newMap.clear();
	openMap.clear();
	openSet.clear();
	closedSet.clear();
	newNodes.clear();
	path.clear();
	target=_target;
	firstVisitor=visit(startNode,target,null);
	searching=true;
}

ASSearch::ASSearch(ASNode*	startNode,ASNode*	_target){
	startSearch(startNode,_target);
}

ASSearch::~ASSearch(){
	visitorPool.reset();
}

ASNode*	findNearestNodeInList(ASNodeList&	nlst,v3	pos,bool	collide=false){
	ASNodeIterator	ito;
	float	bestD=-1.0f;
	ASNode*	best=null;
	for(ito=nlst.begin();ito!=nlst.end();ito++){
		v3	dv=(*ito)->centroid-pos;
		f32 d2=length2(dv);
		if(!best){
			bestD=d2;
			best=*ito;
		}
		else if(d2<bestD){
			if(!collide){
				bestD=d2;best=*ito;
			}else{
				static rayProbe	query;
				v3 s=(*ito)->centroid;
				v3 e=pos;
				v3	obstruction;
				v3	normal;
				if(query.obstructed(s,e,0.0f,obstruction,normal)==false)
				{
					bestD=d2;best=*ito;
				}
			}
		}
	}
	if(best==null){
	int m=0;
	}
	return best;
}

ASNode*	AITask::findNearestNode(v3	pos,bool	collide){
	return findNearestNodeInList(indexLookup(pos),pos,collide);
//	return findNearestNodeInList(gAITask->nodes,pos,collide);
}


void	AITask::start(){
};

void	AITask::stop(){
	ASNodeIterator	ito=nodes.begin();
	while(ito!=nodes.end()){delete *ito;ito++;};
	nodes.clear();
	ASSearchIterator	sito=searches.begin();
	while(sito!=searches.end()){delete *sito;sito++;};
	searches.clear();
};

void	AITask::clearVisited(){
	for(ASNodeIterator	itor=visited.begin();itor!=visited.end();itor++)
		(*itor)->visited=false;
	visited.clear();
};

void	AITask::renderNavMesh(){
	static	c4	islandColors[]={
		c4::blue,
		c4::white,
		c4::green,
		c4::yellow,
		c4::green+c4::red,
		c4::blue+c4::red,
		c4::blue+c4::green,
		c4::ltgray+c4::red,
		c4::ltgray+c4::blue,
		c4::ltgray+c4::green,
	};
	static	int	maxColors=array_size(islandColors);
	for(ASNodeIterator	scn=nodes.begin();scn!=nodes.end();scn++){
		int	cidx=(*scn)->islandIndex%maxColors;
		c4 c=islandColors[cidx];
		c.a[3]=0.5f;
		glColor4fv(&c[0]);
		for(ASNodeIterator	con=(*scn)->links.begin();con!=(*scn)->links.end();con++){
			if((!(*con)->visited)&&(!linkIsDead(*scn,*con))){
				glVertex3fv(&(*scn)->centroid.a[0]);
				glVertex3fv(&(*con)->centroid.a[0]);
			}
		}
		(*scn)->visited=true;
		visited.push_back((*scn));
	}
	clearVisited();
};

bool	AITask::linkIsDead(ASNode*	nn,ASNode*	n){
	map<ASNode*,set<ASNode*>>::iterator	df=deadLinks.find(nn);
	if(df!=deadLinks.end()){if(df->second.find(n)!=df->second.end())return true;}
	df=deadLinks.find(n);
	if(df!=deadLinks.end()){if(df->second.find(nn)!=df->second.end())return true;}
	return false;
}

void	AITask::findIslands(bool	deleteIslands){
	int i=0;
	ASNodeList	islandRoots;
	for(ASNodeIterator	n=nodes.begin();n!=nodes.end();n++)(*n)->islandIndex=i++;

	int	changedCount;
	do{
		//Find all islands by checking node neighbors and swapping with lowest index..
		changedCount=0;
		for(ASNodeIterator	n=nodes.begin();n!=nodes.end();n++){
			bool	changed=false;
			for(ASNodeIterator	nn=(*n)->links.begin();nn!=(*n)->links.end();nn++){	
				//Skip dead links...
				if(linkIsDead(*n,*nn)){
					continue;
				}
				//take index of lower indexed neighbor
				if((*nn)->islandIndex<(*n)->islandIndex){
					(*n)->islandIndex=(*nn)->islandIndex;
					changedCount++;
				}
			}
		}
	}while(changedCount);
	map<int,ASNodeList>	islandMap;
	//Sort all the nodes by islandindex into a map
	for(ASNodeIterator	n=nodes.begin();n!=nodes.end();n++){
		islandMap[(*n)->islandIndex].push_back((*n));
	}
	map<int,ASNodeList>::iterator	mi=islandMap.begin();
	//Show the islands info
	console_printf("Found:%d islands.\n",islandMap.size());
	ASNodeList*	biggestIsland=null;
	while(mi!=islandMap.end()){
		ASNodeIterator	nni=mi->second.begin();
//		while(nni!=mi->second.end()){
//			nni++;
//		}
		if((!biggestIsland)||biggestIsland->size()<mi->second.size())biggestIsland=&mi->second;
//		log_printf("isle:%d size:%d\n",mi->second[0]->islandIndex,mi->second.size(),islandMap.size());
		mi++;
	}

	if(!deleteIslands){
		/*
		for(ASNodeIterator	n=nodes.begin();n!=nodes.end();n++){
			rayProbe	query;
			ASNode*		an=*n;
			if(an->islandIndex!=0){
				//Non water node...
				v3	up(0,-1000,0);
				v3	obstruction;
				if(query.obstructed(an->centroid-up,an->centroid+up,0.0f,obstruction)){
					an->centroid=query.closest->point-(up*0.005f);
				}
			}
		}*/

		return;
	}
	//Delete all the non-main island nodes.
	log_printf("Largest:%d size:%d\n",(*(*biggestIsland).begin())->islandIndex,(*biggestIsland).size());
	ASNodeList	liveList;
	map<ASNode*,bool>	deleted;
	for(ASNodeIterator	n=nodes.begin();n!=nodes.end();n++){
		if((*n)->islandIndex==(*(*biggestIsland).begin())->islandIndex)
			liveList.push_back((*n));
		else{
			if(deleteIslands){
				(*n)->detach();
				delete (*n);
			}
		}
	}
	nodes=liveList;
};

void	AITask::destroyLink(ASNode*	node1,ASNode*	node2)
{
	deadLinks[node2].insert(node1);
	deadLinks[node1].insert(node2);
};


bool	AITask::isWater(ASNode*	a){
	const float fudge=(bounds.max[zaxis]-bounds.min[zaxis])*0.5f;
	if(a->centroid[zaxis]>(bounds.min[zaxis]+fudge))
		return false;
	return true;
}

bool	AITask::isShore(ASNode*	a,ASNode* b){
	bool	aw=isWater(a);
	bool	bw=isWater(b);
	if(aw!=bw)
		return true;
	return false;
}


static	const	int		gdim=64; //128
static	const	float	celSize=1000.0; //128
static	int		gminx=0;
static	int		gminy=0;
static	int		gmaxx=0;
static	int		gmaxy=0;


ASNodeList&	AITask::indexLookup(v3&	pos){
	int	px=((int)pos[2]/celSize);
	int	py=((int)pos[0]/celSize);
	if(py<gminy)py=gminy;
	if(px<gminx)px=gminx;
	if(py>gmaxy)py=gmaxy;
	if(px>gmaxx)px=gmaxx;
	return spatialHash[px+(py*gdim)];
}

void	AITask::expandIndex(v3&	pos){
	int	px=((int)pos[2]/celSize);
	int	py=((int)pos[0]/celSize);
	if(py<gminy)gminy=py;
	if(px<gminx)gminx=px;
	if(py>gmaxy)gmaxy=py;
	if(px>gmaxx)gmaxx=px;
}

void	AITask::generateNavMesh(BBox&	worldBounds,bool castSign,bool	checkObstructions){
	root = null;
	v3			s;
	v3			e;
	float		d=1.0f/gdim;
	float		wscale=300.0f;
	float		fudgeRatio=0.999f;
	v3			wdim=worldBounds.dimensions()*fudgeRatio;
	v3			fdim=wdim*(1.0f-fudgeRatio);
	float		dz=wdim[zaxis];
	float	padZOffset=0;//100.0;
	//worldBounds

	ASNode		**grid=new ASNode*[gdim*gdim];
	rayProbe	query;
//	bool		checkObstructions=false;
	v3			obstruction;
	v3			normal;
//	bool		castNegative=true;
	//Drape a grid mesh over the terrain.
	for(int	tx=0;tx<gdim;tx++){
		for(int	ty=0;ty<gdim;ty++){
			ASNode*	gn=new ASNode();
			nodes.push_back(gn);
			grid[(ty*gdim)+tx]=gn;
			float	px=((float)tx/gdim);//-fdim[xaxis];
			float	py=((float)ty/gdim);//-fdim[yaxis];
			v3		zray=v3((wdim[xaxis]*px)+worldBounds.min[xaxis]+fdim[xaxis],
							worldBounds.max[zaxis],
							(wdim[yaxis]*py)+worldBounds.min[yaxis]+fdim[yaxis]);
			v3		zrend=zray;
			zray[zaxis]-=dz;
			if(!castSign){
				v3	swp=zrend;
				zrend=zray;
				zray=swp;
			}
			gn->centroid=v3(zrend);
			
			expandIndex(gn->centroid);
			
			indexLookup(gn->centroid).push_back(gn);

			if(query.obstructed(zrend,zray,0.0f,obstruction,normal)){
				gn->centroid=obstruction;
				gn->centroid[zaxis]+=padZOffset;
				gn->normal=normal;
			}
			if(tx==0&&ty==0){
				bounds=gn->centroid;
			}else{
				bounds.accumulate(gn->centroid);
			}
		}
	}
	//Check all links for obstructions or shoreline
	for(int	tx=0;tx<gdim;tx++){
		for(int	ty=0;ty<gdim;ty++){
			ASNode*	gn=grid[tx+(ty*gdim)];
			//Connect horizonal and verticals
			if(ty>0){
				ASNode*	ly=grid[tx+((ty-1)*gdim)];
				if(isShore(gn,ly)||
					(checkObstructions&&
					query.obstructed(gn->centroid,ly->centroid,0.0f,obstruction,normal)))
				{
					destroyLink(gn,ly);//Add this link to the destroy map
				}
				gn->linkTo(ly);
			}
			if(tx>0){
				ASNode*	lx=grid[(tx-1)+(ty*gdim)];
				if(isShore(gn,lx)||
					(checkObstructions&&query.obstructed(gn->centroid,lx->centroid,0.0f,obstruction,normal))){
					destroyLink(gn,lx);//Add this link to the destroy map
				}
				gn->linkTo(lx);
			}
			if((tx>0)&&(ty>0)){
				//Connect diagonals
				ASNode*	l1=grid[(tx-1)+((ty-1)*gdim)];
				ASNode*	l2=grid[(tx  )+((ty-1)*gdim)];
				ASNode*	l3=grid[(tx  )+((ty  )*gdim)];
				ASNode*	l4=grid[(tx-1)+((ty  )*gdim)];
				v3	obstruction;
				if(isShore(l1,l3)||
					(checkObstructions&&query.obstructed(l1->centroid,l3->centroid,0.0f,obstruction,normal)))
					destroyLink(l1,l3);
				
				l1->linkTo(l3);

				if(isShore(l2,l4)||
					(checkObstructions&&query.obstructed(l2->centroid,l4->centroid,0.0f,obstruction,normal)))
					destroyLink(l2,l4);

				l2->linkTo(l4);
			}
		}
	}
	delete grid;
}


void	ASSearch::renderPath(){
	static	c4	ctable[256];
	static	bool	ctbuilt=false;
	if(!ctbuilt){
		ctbuilt=true;
		for(int t=0;t<array_size(ctable);t++){
			ctable[t]=(c4::random()*0.5f)+0.5f;
		}
	}
	if(!gSimTask->debugDrawingEnabled())
		return;
	if(path.size()){
		c4	clr=ctable[(path.size()%256)];
		Renderer::color(clr);
		list<ASNodeVisitor*>::iterator	sp=path.end();
		Renderer::pushMatrix();
		Renderer::translate(0,100.0f,0);
		glBegin(GL_LINES);
		for(list<ASNodeVisitor*>::iterator p=path.begin();
			p!=path.end();
			p++){
			if(sp!=path.end()){
				
				glVertex3fv(&(*sp)->node->centroid.a[0]);
				glVertex3fv(&(*p)->node->centroid.a[0]);
			}
			sp=p;
		}
		glEnd();
		Renderer::popMatrix();
	}
}

ASNode*			AITask::getNode(int index){
	ASNodeIterator ai=nodes.begin();
	advance(ai,index);
	return *ai;
}

ASNode*			AITask::getRandomNode(){
	return getNode(rand()%nodes.size());
}

void	AITask::render(){
	static	GLuint	meshDisplayList = -1;
	static	ASSearch*	search=null;
	float	tm=perfTimer.getTime();
	static	float	nextTime=0;
	static	ASNode* start=null;
	static	ASNode* end=null;

	if(meshDisplayList==-1 && nodes.size()){
		meshDisplayList = glGenLists(1);
		glNewList(meshDisplayList,GL_COMPILE);
		glBegin(GL_LINES);
		renderNavMesh();
		glEnd();

		glColor4f(1,1,1,1);
		renderBox(bounds);

		glEndList();
	}
	if(nodes.size()){
		Renderer::texturing(false);
		Renderer::lighting(false);
		Renderer::depthTest(false);
		Renderer::depthMask(true);
		Renderer::blending(true);
	Renderer::polygonMode(Renderer::WIRE,Renderer::WIRE);
	Renderer::cullMode(Renderer::NONE);

		glColor4f(1,0,0,1.0f);
		
		glCallList(meshDisplayList);

		if(search)
		{	
			glPolygonOffset(-1,-1);
			search->renderPath();
			glPolygonOffset(0,0);
		}

	Renderer::cullMode();
	Renderer::polygonMode();

		Renderer::depthMask();
		Renderer::depthTest();
		glColor4f(1,0,0,1.0f);	
//		glCallList(meshDisplayList);

		Renderer::blending();
		Renderer::texturing();
		Renderer::lighting();		
		glColor4f(1,1,1,1);
	}

}

void	AITask::run()
{
	if(gSimTask->debugDrawingEnabled())Renderer::addToRenderList(this);
};

void	ASSearch::pathToArray(Array<ASNode*>&	nar){
	list<ASNodeVisitor*>::iterator	i=path.begin();
	int	c=0;
	nar.resize(path.size());
	while(i!=path.end())
		{nar[c++]=(*i)->node;i++;}
}

ASNode*	ASNode::randomWalk(int	steps)
{
	ASNode*	scn=this;
	set<ASNode*> visited;
	visited.insert(this);
	int	dir=rand()&1;
	while(steps&&scn->islandIndex==islandIndex){
		for(u32 t=0;t<scn->links.size();t++){
			ASNode*	lk=*(scn->links.begin()+(dir?t:(scn->links.size()-1)-t));
			if(visited.find(lk)==visited.end()&&(lk->islandIndex==scn->islandIndex)){
				scn=lk;
				visited.insert(scn);
				break;
			}
		}
		steps--;
	}
	return scn;
}


AITask::AITask(const char*	mapName,bool	deleteIslands,bool	castNegative,bool	checkObstructions){
	root=null;
	meshDensity=1.0f/128;
	if(nodes.size()==0){
		string	aiMeshName = RESOURCE_DIRECTORY+string("\\cache\\")+mapName+string(".ai");
		string	sourceName = RESOURCE_DIRECTORY+string("\\")+mapName+string(".scene");
		bool	rebuild=false;
		if(!fileExists(aiMeshName.c_str()))rebuild=true;
		else if(fileIsNewer(sourceName.c_str(),aiMeshName.c_str()))rebuild = true;

		if(fileIsNewer("src\\AStar.cpp",aiMeshName.c_str())){
			rebuild=true;
		}
		if(rebuild){
			Time t=perfTimer.getTime();
			generateNavMesh(gSimTask->staticBounds,castNegative,checkObstructions);
			Time gt=perfTimer.getTime();
			findIslands(deleteIslands);
			Time ft=perfTimer.getTime();
			log_printf("Gen T:%f  Isle T:%f\n",gt-t,ft-gt);
			map<ASNode*,int>			nodeMap;
			map<ASNode*,int>::iterator	nmi;
			int	i=0;

			FILE*	out=null;
			Dictionary	dict;
			ASNode	tmp;
			tmp.getDescription(dict);
			if(fopen_s(&out,aiMeshName.c_str(),"wt")==0){
				string m=format("%d,\n",nodes.size());
				fprintf(out,m.c_str());
				ASNodeIterator	ni=nodes.begin();
				while(ni!=nodes.end()){nodeMap[*ni]=i;ni++;}
				
				for(ni=nodes.begin();ni!=nodes.end();ni++)nodeMap[*ni]=i;

				for(ni=nodes.begin();ni!=nodes.end();ni++)
					fprintf(out,"%08X,\n",(void*)(*ni));

				for(ni=nodes.begin();ni!=nodes.end();ni++){
					Describable::writeObject(out,dict,(void*)(*ni));
					ASNodeIterator lnks=(*ni)->links.begin();
					string m=format("%d,\n",(*ni)->links.size());
					fprintf(out,m.c_str());
					while(lnks!=(*ni)->links.end()){
						m=format("%08X,\n",(*lnks));
						fprintf(out,m.c_str());
						lnks++;
					}
				}
				fclose(out);
			}
		}else{
			const int	MX_NUM=256;
			FILE*	in=null;
			Dictionary	dict;
			ASNode	tmp;
			tmp.getDescription(dict);
			if(fopen_s(&in,aiMeshName.c_str(),"rt")==0){
				
				char	nmbuf[MX_NUM];
				fgets(nmbuf,MX_NUM,in);
				Describable::stripcrlf(nmbuf,strlen(nmbuf));
				int	nodeCount=atoi(nmbuf);
				map<void*,int>	pxl;
				for(int ni=0;ni<nodeCount;ni++){
					fgets(nmbuf,MX_NUM,in);
					Describable::stripcrlf(nmbuf,strlen(nmbuf));
					void*	ptr;
					sscanf_s(nmbuf,"%08X",&ptr);
					pxl[ptr]=ni;
				}
				for(int ni=0;ni<nodeCount;ni++){
					ASNode*	an=new ASNode();
					nodes.push_back(an);
					Describable::readObject(in,dict,(void*)an);
					fgets(nmbuf,MX_NUM,in);
					Describable::stripcrlf(nmbuf,strlen(nmbuf));
					int	lnkCount=atoi(nmbuf);
					ASNodeList	links;
					for(int l=0;l<lnkCount;l++){
						fgets(nmbuf,MX_NUM,in);
						Describable::stripcrlf(nmbuf,strlen(nmbuf));
						ASNode*	ptr;
						sscanf_s(nmbuf,"%08X",&ptr);
						an->links.push_back(ptr);
					}
				}
				ASNodeList	links;	
				
				for(ASNodeIterator	scn=nodes.begin();scn!=nodes.end();scn++){
					ASNode*	nd=*scn;
					indexLookup(nd->centroid).push_back(nd);
					int	lnkCount=nd->links.size();
					ASNodeIterator lnk=nd->links.begin();
					for(int li=0;li<lnkCount;li++){
//						advance(lnk,li);
						links.push_back(getNode(pxl[(void*)(*lnk)]));

						lnk++;
					}
					nd->links=links;
					links.clear();
				}
				fclose(in);
			}
		}
	}
};

AITask::~AITask(){
};

